#include <ros/ros.h>
#include <string>
#include <std_msgs/Header.h>
#include <geometry_msgs/Pose.h>
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/OccupancyGrid.h>
#include "ares_pathing/aresMap.h"

#ifndef _ARES_MAIN
#define _ARES_MAIN
using namespace tf;
namespace pathing{

static ros::Subscriber sub_map;
static ros::Subscriber sub_odom;
static ros::Subscriber sub_goal;
static boost::shared_ptr<tf::TransformListener> tfl;
static ros::Publisher pub_path;

aresMap * world;
bool ready = false;
bool mapflag = false;
bool odomflag = false;
bool goalflag = false;

/* INLINE
Preconditions: an occupancy_grid message arrives on the projected_map topic
Postconditions: make a new aresMap out of the message
*/
void mapCallback(const nav_msgs::OccupancyGrid::ConstPtr& msg){
    ROS_INFO("In map callback");
    if(!mapflag){ 
        world = new aresMap(*msg);
        ROS_INFO("world saved");    
    }
    else{
        world->updateMap(*msg);
        ROS_INFO("world updated");    
    }
    mapflag = true;
}

/* INLINE
Preconditions: an odometry message arrives on the Odometry topic
Postconditions: useful parts of the message are saved for later use
*/
void odomCallback(const nav_msgs::Odometry::ConstPtr& odomMsg){
    ROS_INFO("In odom callback");
    if(!mapflag){
        ROS_WARN("No map to set odom in");
        return;
    }
    world->updateBot(*odomMsg);
    odomflag = true;
}

/* INLINE
Preconditions: a geometry_msgs::PoseStamped message arrives on the move_base_simple/goal topic
Postconditions: the pose is sent to the map to be set as the goal
*/
void goalCallback(const geometry_msgs::PoseStamped::ConstPtr& goalMsg){
    ROS_INFO("In goal callback");    
    ROS_INFO("Goal: (%f,%f) - Rot: %f",goalMsg->pose.position.x,goalMsg->pose.position.y,tf::getYaw(goalMsg->pose.orientation));    
    if(!mapflag){
        ROS_WARN("No map to set goal for");
        return;
    }
    world->setGoal(goalMsg->pose);
    goalflag = true;
}

void drivePub();
};//end namespace

#endif
